阿基米德螺旋线路径功能开发介绍

1 背景介绍

在机械臂装配搜孔的过程中，需要高效率地进行无碰撞的路径规划。为了满足这一需求，阿基米德螺旋线路径被引入作为一种优化的运动轨迹。阿基米德螺旋线路径的功能开发旨在提供以下关键特性：

• 无碰撞路径规划

阿基米德螺旋线路径具有连续性和平滑性，能够有效避免与环境中其他物体发生碰撞。通过使用这种路径，可以最小化机械臂在移动过程中的危险，并确保装配任务的安全性。

• 连续平滑运动

螺旋线路径是一种连续平滑的运动轨迹，这有助于减小机械臂在运动过程中的振动。这对于需要高精度装配的应用场景尤为重要，因为振动可能导致不准确的位置和损坏零件。

• 自补偿特性

阿基米德螺旋线路径具有自补偿的特性，即在旋转的同时也在向前推进。这种自补偿的特性可以减少在运动过程中的额外操作，提高装配效率。

• 适应性和灵活性

该功能提供了调整螺旋线参数的选项，包括螺距、半径等，以适应不同大小和形状的孔，这增加了路径规划的灵活性，使其适用于各种装配场景。

2 运动说明

• 螺旋运动由三个点确定：起始点，给定第一点（圆心），给定第二点（确定平面的点）；给定第一点确定初始圆心（螺旋运动中心），也确定了初始圆半径，给定第二点用于确定螺旋运动的基准平面并给定螺旋运动终点姿态（该点可在机器人工作空间的确定平面内任意给出，不与前面两点重合或过于接近即可，在运动过程中由起始点姿态运动到终点姿态）

  给定圆心与起始点不能重合或过于接近，否则会报错，给定圆心距离起始点不要小于1mm

• 螺旋由起始点开始运动（非圆心处），以设置的步长stepsize进行运动，分正负值，正值为外扩运动，负值为内缩运动（若给定螺旋角度过大，终点设定为圆心处），设置为零则退化为圆弧运动；以设置的alpha值作为螺旋运动终点，即螺旋运动几圈，分正负值，正值为逆时针运动（以参考系z轴为基准），负值为顺时针运动；growthrate为螺旋平面垂直方向的步长，分正负值，默认正值为垂直螺旋平面上升方向，负为垂直螺旋平面下降方向，下方螺旋运动示意图中为负值，若未设置growthrate值，默认为平面螺旋；若设置stepsize为零并且growthrate值不为零，则为柱状螺旋运动

• 螺旋运动支持姿态摆动，摆动幅度为设置的belta值，默认为0，不进行姿态摆动；一个摆动周期分为三部分运动，首先正方向运动1/4个周期到达正方向幅值最大处，然后反向运动1/2个周期到达负方向幅值最大处，最后正方向运动1/4个周期到达摆动起始处；摆动频率为一个周期数值的倒数，为单位弧长所摆动的次数（保证每个周期摆动速率相同），见下方姿态摆动示意图

• 由于螺旋线运动方向与用户坐标系相关，用户坐标系的建立与起始点以及传入的两个点顺序相关，所以给定三点用户坐标系的建立标准：

  1.以第一个示教点为圆心建立参考坐标系，将该坐标系设为用户坐标系

  2.将初始点连接到第一个示教点，得到 x 轴正方向

  3.将第一个示教点和第二个示教点连线作为过渡轴方向

  4.给定通过右手定则判断 z 轴方向，跟据z轴与x轴确定y轴方向，从而确定用户坐标系的方向

  示教时，需要确定第一个示教点和初始点连线与第一个示教点和第二个示教点连线夹角 $\theta$的范围：若0°< 夹角 <180°，则螺旋运动坐标系z轴朝上；夹角>180°，则z轴朝下，螺旋运动正方向为逆时针运动（alpha >0），这里顺逆时针判定的基础是从 z 轴的朝向俯视来看，具体可参照下图，图示假定三个点均在x，y平面内

3 接口介绍

    /**
     * @brief 向规划器中添加螺旋线路径: 
     * 螺旋线路径不支持交融，暂停/恢复和动态调速
     * @param points: 描述路径几何信息的路径点，一般至少3个特征点，其中第一个点为机械臂当前点，剩余两个点通过points传入，第二个点作为螺旋线的起始圆心，第三个点作为确定螺旋运动平面的点
     * @param path_property: 路径属性: 见结构体 ArchimedeanspiralParameters 和 SuperimposedParameters
     * @param move_property: 路径对应的运动属性
     * @return: 返回值 < 0 表示添加失败, 具体定义查询return_value_definition.hpp
     */
    ARAL_API_COMMON(1.0) int tpAddPoints(const std::vector<PathPoint>& points, const PathProperty& path_property, const MoveProperty& move_property) = 0;

struct ArchimedeanspiralParameters
{
    double stepsize;                       // 每转一圈螺旋运动对应圆半径增加的步长(正值为外扩，负值为内缩)
    double alpha;                          // 螺旋运动角度范围(正值为逆时针，负值为顺时针)
    double growthrate{0};                  // 每转一圈螺旋运动对应z轴方向增加的步长(正值为上升，负值为下降)
}; // 阿基米德螺旋线结构体

struct SuperimposedParameters
{
    bool        enable{false};          // 使能路径叠加功能(在运动路径的法平面内进行叠加)
    WeaveType   type;                   // 路径类型(针对焊接应用, 就是摆动类型,比如正弦,摆线)
    double      step;                   // 一个周期内在运动方向上移动的距离(摆动步长)
    Vector2d    amplitude;              // 一个周期内在运动叠加方向的最大幅值(不一定对称),当复用为摆线的幅值时,只用第一个元素
    Vector2d    holdDistance;           // 运动到最大幅值处保持的距离(不一定对称),当复用为摆线的宽度时,只用第一个元素
    double      angle;                  // 运动叠加方向与法平面的夹角
    int         direction{0};           // 摆线起始摆动方向: 0表示在路径方向上方,1表示在路径方向下方
    double      belta{0};               // 姿态摆动范围（0～belta）:默认为0不进行姿态摆动
    double      frequency;              // 姿态摆动频率(单位弧长对应的摆动次数)  范围: [0, 1]
}; // 叠加路径属性

4 开发例程

        auto type = getTestType();
        Setup(getRobotType(type.second));
        interface::MoveProperty move_property;
        std::vector<interface::RLJntArray> joints;
        std::vector<interface::RLPose> poses(2);
        interface::RLPose pose1;
        interface::RLJntArray q0;
        interface::PathProperty path_property = generatePathProperty({0, 0}, ARAL_TP_DEFAULT_BOW_HIGH_ERR, interface::DescribeSpace::CARTESIAN);
        double PI = 3.1415926;
        int N = 2;
        if(type.first == 1)    // 搜孔装配 1.快速定位阶段不需要姿态摆动，螺旋方向由给定的第二个点确定
        {
            q0 = {-0.00229631,-0.520184,1.99254,0.942027,1.5707,-0.0342557};
            robot->kdCalForwardPosition(q0, true, true, pose1); // 起始点位姿
            std::vector<double> target_circle_center_error = { 0.0, 0.001, 0, 0.0, 0.0, 0.0 }; 
            std::vector<double> target_xy_error = { 0.02, 0.02, 0, 0.0, 0.0, 0.0 }; 
            for (int i = 0; i < 6; i++)
            {
                poses[0][i] = pose1[i] + target_circle_center_error[i]; // 螺旋运动起始圆圆心位置
                poses[1][i] = pose1[i] + target_xy_error[i]; // 确认螺旋运动平面的第二个点(这里指定xy平面为运动平面)
            }

            move_property.maxV = {0.3, 0.8};
            move_property.maxA = {4.0, 20.0};
            move_property.maxJ = {move_property.maxA[0] * 8, move_property.maxA[1] * 8};

            path_property.curProp.type = interface::CurveType::ARCHIMEDEAN_SPIRAL;
            path_property.curProp.ap.stepsize = 0.0025;      // 螺旋步长 2.5 mm，外扩
            path_property.curProp.ap.alpha = 4 * PI;         // 螺旋角度范围（2PI为一个周期，即两圈螺旋运动，逆时针）
            path_property.curProp.ap.growthrate = -0.005;    // 螺旋运动在z轴方向上的步长 5 mm(装配时向下运动)
        }
        else                   // 搜孔装配 2.搜孔阶段需要姿态摆动
        {
            q0 = {-0.00229631,-0.520184,1.99254,0.942027,1.5707,-0.0342557};
            robot->kdCalForwardPosition(q0, true, true, pose1);
            std::vector<double> target_circle_center_error = { 0.0, 0.001, 0, 0.0, 0.0, 0.0 };
            std::vector<double> target_xy_error = { 0.02, 0.02, 0, 0.0, 0.0, 0.0 };
            for (int i = 0; i < 6; i++)
            {
                poses[0][i] = pose1[i] + target_circle_center_error[i]; // 螺旋运动起始圆圆心位置
                poses[1][i] = pose1[i] + target_xy_error[i]; // 确认螺旋运动平面的第二个点(这里指定xy平面为运动平面)
            }

            move_property.maxV = {0.3, 0.8};
            move_property.maxA = {2.5, 3.0};
            move_property.maxJ = {move_property.maxA[0] * 8, move_property.maxA[1] * 8};

            path_property.curProp.type = interface::CurveType::ARCHIMEDEAN_SPIRAL;
            path_property.curProp.ap.stepsize = 0.001;      // 螺旋步长 1 mm
            path_property.curProp.ap.alpha = -4 * PI;       // 螺旋角度范围（2PI为一个周期，即两圈螺旋运动，顺时针）
            path_property.curProp.ap.growthrate = -0.0005;  // 螺旋运动在z轴方向上的步长 0.5 mm(装配时向下运动)
            path_property.curProp.sp.enable = true;         // 使能路径叠加功能
            path_property.curProp.sp.belta = PI / 90;       // 姿态摆动幅度 2 度
            path_property.curProp.sp.frequency = 0.5;       // 姿态摆动频率(单位弧长(cm)对应的摆动次数) 范围: [0, 1]
        }
        initiatePlanner(robot, q0, tcp);

        std::vector<interface::PathPoint> points(N);
        for(int i = 0; i < N; i++)
            points[i] = generatePathPoint(interface::DescribeSpace::CARTESIAN, i+1, {}, poses[i]);

        int ret = robot->tpAddPoints(points, path_property, move_property);
        CHECK(ret >= 0);

        std::vector<interface::TrajectoryPoint> plan_points;
        std::vector<interface::RLJntArray> command_pos, command_vel, command_acc;
        std::vector<double> t;
        ret = generateTrajectoryPoints(robot, q0, 0.005, plan_points, command_pos, command_vel, command_acc, t);
        CHECK(ret >= 0);

        plot(plan_points, command_pos, command_vel, command_acc, t);